///////////////////////////////////////////////////////////////////////////////////////////////
/// This is a header file to be used with Team Vicorty's   ///////////////////////////////////
/// Mercury Robot. It has the functions that are needed    ///////////////////////////////////
///	to contorl the motors on the robot. QRD1113 IR transmit///////////////////////////////////
/// receive package is connected to a Schmidt Trigger      ///////////////////////////////////
/// inverter that is connected to digital inputs on a      ///////////////////////////////////
///	ATMega324P microcontroller. 						   ///////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
/// Created by Jonathan Kropp		                       ///////////////////////////////////
///	Version		Date: 							 		   ///////////////////////////////////
///  V.0		2/29/2012                        		   ///////////////////////////////////
///												 		   ///////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
	
#define	left_encoder PC7
#define right_encoder PC6

volatile int turns = 0;
volatile int turnsL = 0;
volatile int turnsR = 0;
volatile uint8_t oldstate;
volatile uint8_t newstate;

void init_Encoders(void){

	EICRA|=0b00000001;					// any edge triggers interrupt
	PCICR|=0b00000100;					// enable Pin Change Interrupt 2
	PCMSK2|=0b11000000;					// PC7(PCINT23) and PC6(PCINT22) pin change interrupt enabled 
					

	set_input(DDRC,left_encoder);		// set encoder pins as inputs
	set_input(DDRC,right_encoder);

	//PORTC|=0b11000000;					// set PC7 and PC6 to known low state

	oldstate = 0;
	newstate = 0;


}


ISR(PCINT2_vect){
	turns++;
	
	newstate = PINC&0b11000000;
	uint8_t temp = newstate^oldstate;
	
	if(temp&0b10000000){
		turnsL++;
	}
	if(temp&0b01000000){
		turnsR++;
	}
	/*if(turnsL >= 500){
		highBrake();
		printf("Stop!, turnsL = %d, turnsR = %d, oldstate = %x, newstate = %x\n", turnsL, turnsR, oldstate, newstate);

		turnsL = 0;
		turnsR = 0;
		turns = 0;
	}*/

	oldstate = newstate;
}
	 
